#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wcast-qual"
#pragma GCC diagnostic ignored "-Wzero-as-null-pointer-constant"
#pragma GCC diagnostic ignored "-Wenum-compare"
#pragma GCC diagnostic ignored "-Wdouble-promotion"
#pragma GCC diagnostic ignored "-Wunknown-pragmas"
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#include <buola/cv/ccloudview.h>
#include <buola/app/ceventloop.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <buola/mat/transformations.h>
#include <buola/cv/coccupancyoctree.h>
#include <buola/mat/cgaussianprocess.h>
#pragma GCC diagnostic pop

using namespace buola;

int main(int pNArg,char **pArgs)
{
    pcl::PointCloud<pcl::PointXYZRGBA> lPCL;

    start_timer();
    pcl::io::loadPCDFile((io::get_home()/"tabletop.pcd").Path(),lPCL);
    end_timer();

    mat::CVec3d l3{0.349425,0.13863,0.997};
    mat::CVec3d l4{-0.0957829,0.155234,0.867};
    mat::CVec3d l5{-0.0709429,0.2292,0.573};

    mat::CVec3d lCross=normalize(cross(l5-l3,l4-l3));

    mat::CVec3d lTableX{1,0,0};
    mat::CVec3d lTableZ=-normalize(lCross);
    mat::CVec3d lTableY=normalize(cross(lTableZ,lTableX));
    lTableX=cross(lTableY,lTableZ);

    mat::CVec3d lTableO=l3-lTableX*0.55-lTableY*0.6;

    mat::CMat34d lTrans=mat::transform_3d(lTableO,lTableX,lTableY,lTableZ);
    mat::CMat34d lTransI=inverse(lTrans);

    msg_info() << lTrans << lTransI << "\n";

    msg_info() << lTransI*mat::CVec3d(lTableO+lTableX) << "\n";
    
    msg_info() << lTableX << lTableY << lTableZ << "\n";
    
    buola_init(pNArg,pArgs);

    try
    {
        cvi::CCloud_rgb8 lCloud;
        start_timer();
        for(int i=0;i<lPCL.size();i++)
        {
            if(!is_nan(lPCL[i].x))
            {
                cvi::CCloud_rgb8::TPoint lPoint;
                lPoint.mPos={lPCL[i].x,lPCL[i].y,lPCL[i].z};
                lPoint.mPos=lTransI*lPoint.mPos;
                if(lPoint.mPos.x()<0||lPoint.mPos.x()>1||lPoint.mPos.y()<0||lPoint.mPos.y()>1||lPoint.mPos.z()<-0.1) continue;
                lPoint.mData=img::pixel(lPCL[i].r,lPCL[i].g,lPCL[i].b);
                lCloud.push_back(lPoint);
            }
        }
        end_timer();

        lCloud.SetOrigin(lTransI*mat::CVec3d(0,0,0));

        cvi::COccupancyOcTree lTree(0.003);

        start_timer();
        lTree.InsertCloud(lCloud);
        end_timer();

        for(auto i=lTree.begin();i!=lTree.end();++i)
        {
            cvi::CCloud_rgb8::TPoint lP;

            lP.mPos=i.Coords();

            if(i->Value()>0)
            {
                lP.mData[0]=255;
                lP.mData[1]=0;
                lP.mData[2]=0;
            }
            else
            {
                lP.mData[0]=127;
                lP.mData[1]=127;
                lP.mData[2]=127;
            }

            lCloud.push_back(lP);
        }

        start_timer();
        mat::CMat_d lHeightMap=cvi::get_height_map_mat(lTree,{100,100},0,0.6,0,0.6,-0.1,0.5);
        end_timer();

        int r=0;
        for(int x=0;x<100;x++)
        {
            for(int y=0;y<100;y++)
            {
                if(is_nan(lHeightMap(x,y))) continue;
                ++r;
            }
        }
        msg_info() << r << "\n";
        mat::CMat_d lPoints(r,2);
        mat::CVec_d lValues(r);

        img::CImage_gray8 lHMapImage({100,100});

        for(int x=0;x<100;x++)
        {
            for(int y=0;y<100;y++)
            {
                    lHMapImage(x,y)=clamp(50+500*lHeightMap(x,y),0.0,255.0);
            }
        }

        mat::CVec3d lTallest(0,0,-INFINITY);
        
        save(lHMapImage,io::file("height_map.ppm"));
        r=0;
        for(int x=0;x<100;x++)
        {
            for(int y=0;y<100;y++)
            {
                if(is_nan(lHeightMap(x,y))) continue;
                lPoints(r,0)=x*0.006;
                lPoints(r,1)=y*0.006;

                lValues[r]=lHeightMap(x,y);

                ++r;
                
                if(lHeightMap(x,y)>lTallest.z())
                {
                    lTallest=mat::CVec3d(x*0.006,y*0.006,lHeightMap(x,y));
                }
            }
        }

        mat::CMat34d lKinectToRobot=mat::rotation_translation_3d(mat::CQuat_d(-0.18138060909146622,0.20938222550806626,0.76739448403667976,-0.57824377571831764),
                        mat::CVec3d(0.56611077880859375,1.227518310546875,0.3039049072265625));

        mat::CVec3d l20down=lKinectToRobot*(lTrans*mat::CVec3d(lTallest+mat::CVec3d(0,0,-0.2)));
        msg_info() << "20 cms down:\n" << l20down << "\n";
        mat::CVec3d l20right=lKinectToRobot*(lTrans*mat::CVec3d(lTallest+mat::CVec3d(0.2,0,0)));
        msg_info() << "20 cms right:\n" << l20right << "\n";
        
        msg_info() << "tallest point in table space: \n" << lTallest << "\n";
        lTallest=lTrans*lTallest;
        msg_info() << "tallest point in kinect space: \n" << lTallest << "\n";
        
        lTallest=lKinectToRobot*lTallest;
        msg_info() << "tallest point in robot space: \n" << lTallest << "\n";
        
        msg_info() << "size of height map:" << r << "\n";

        msg_info() << "training gaussian process\n";
        start_timer();
        mat::CGaussianProcess<double> lProcess(lPoints,lValues,1073.0,0.0013,0.04);
        end_timer();
        msg_info() << "log-likelihood=" << lProcess.LogLikelihood() << "\n";
        mat::CMat_d lPredictedMap(100,100);
        msg_info() << "estimating\n";
        for(int x=0;x<100;x++)
        {
            for(int y=0;y<100;y++)
            {
                lPredictedMap(x,y)=lProcess.F(mat::CVec2d({x*0.006,y*0.006}));
            }
        }

        mat::CMat_d lVarianceMap(100,100);
        msg_info() << "estimating\n";
        for(int x=0;x<100;x++)
        {
            for(int y=0;y<100;y++)
            {
                lVarianceMap(x,y)=lProcess.V(mat::CVec2d({x*0.006,y*0.006}));
            }
        }

        
        img::CImage_gray8 lMapImage({100,100});

        
        for(int x=0;x<100;x++)
        {
            for(int y=0;y<100;y++)
            {
                lMapImage(x,y)=clamp(50+500*lPredictedMap(x,y),0.0,255.0);
            }
        }

        
        msg_info() << "variance-map:\n" << lVarianceMap << "\n";
        
        save(lMapImage,io::file("predicted_map.ppm"));

        img::CImage_gray8 lVMapImage({100,100});

        for(int x=0;x<100;x++)
        {
            for(int y=0;y<100;y++)
            {
                    lVMapImage(x,y)=clamp(5000*lVarianceMap(x,y),0.0,255.0);
            }
        }

        
        
        save(lVMapImage,io::file("variance_map.ppm"));

        msg_info() << "height-map:\n" << lHeightMap << "\n";
        
        msg_info() << "log-likelihood=" << lProcess.LogLikelihood() << "\n";
        
        gui::PWindow lWindow=new gui::CWindow;
        lWindow->Create(nullptr,CSize_d(800,600));
        cvi::PCloudView lView=new cvi::CCloudView;
        lView->Create(&lWindow);
        lView->SetAxes({0,0,0},{1,0,0},{0,1,0},{0,0,1});
        mat::CVec3d lCenter=center(lCloud);
        lView.SetCloud(std::move(lCloud));
        lView.SetCameraPos(lCenter-mat::CVec3d({30,0,0}));
        attach(&lView,gui::ATTACH_REPLACECLIENT);
        lView.Show();
        lWindow.Show();

        set_main_window(&lWindow);

        get_main_event_loop().Loop();
    }
    catch(std::exception &pE)
    {
        msg_info() << "caught exception " << pE.what() << "\n";
    }

    return buola_finish();
}
